#include <pthread.h>

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <assert.h>

#define ABS(number) ((number<0)?(-(number)):(number))

/* variables for barrier */
pthread_mutex_t bLock;
unsigned int bLimit;
unsigned int bCount;
unsigned int bIteration;

double **initA, *initB;
double **matA, *matB;
unsigned int *matMax;
unsigned int n;
unsigned int nThreads;

int (*thread_search[5])(int, int, double**);
void (*thread_swap[5])(int, int, int, int, double**);
void (*thread_forward[5]) (int, int, double**, double*);
void (*thread_make_leading_one) (int, int,double**);
void (*thread_backward) (int, int, double**, double*);

/* The following two functions implement a barrier.*/
void init_barrier(int max)
{
	bLimit = max;
	bCount = 0;
	bIteration = 0;
	pthread_mutex_init(&bLock,0);

}

void barrier()
{
	unsigned int myIteration;
	unsigned int myCount;

	myIteration = bIteration;

	pthread_mutex_lock(&bLock);
	myCount = ++bCount;
	pthread_mutex_unlock(&bLock);

	if(myCount == bLimit) {
		bCount = 0;
		bIteration++;
	}   

	while(bIteration == myIteration);
}


#ifdef __DEBUG
void print_matrix(double **mat, int n)
{
	unsigned int i, j;
	printf("------------------------------------\n");
	for (i=0;i<n;++i){
		for (j=0;j<n;++j){
			printf("%.5lf ", mat[i][j]);
		}
		printf("\n");
	}
}
#endif

// for given interval, return the index of maximal element
int thread_search0(int id, int c, double** local_matA)
{
	register double** matA = local_matA;
	register unsigned int iter_begin =c+(id*(n-c))/nThreads;
	register unsigned int iter_end = c+((id+1)*(n-c))/nThreads;

	register unsigned int i;
	register double max = ABS(matA[iter_begin][c]);
	register double tmp;
	unsigned int maxi = iter_begin;
	for (i = iter_begin + 1; i < iter_end; ++i){
		tmp = ABS(matA[i][c]);
		if (max < tmp){
			maxi = i;
			max = tmp;
		}
	}
	return maxi;
}

// swap row r1 and r2 starting with c
void thread_swap0(int id, int r1, int r2, int c,double** local_matA)
{
	register unsigned int i;
	unsigned int m = (int)(((id+1)*(n-c))/nThreads) -(int)((id*(n-c))/nThreads);
	register double t;
	unsigned int iter_begin =c+(id*(n-c))/nThreads;
	register double * matAr1 = local_matA[r1]+iter_begin;
	register double * matAr2 = local_matA[r2]+iter_begin;

	for (i=m; i>0; --i, ++matAr1, ++matAr2){
		t = *matAr1;
		*matAr1 = *matAr2;
		*matAr2 = t;
	}
	if (id == nThreads-1){
		t = matB[r1];
		matB[r1] = matB[r2];
		matB[r2] = t;
	}
}

// forward subtracing
void thread_forward0 (int id, int c, double** local_matA, double* local_matB) // id : thread id, c : (pivot) column
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA;
	register double* matAc;
	double* matB = local_matB+iter_begin;
	register double** origA = local_matA;
	register unsigned int i;
	register unsigned int j;
	double bC = local_matB[c];
	register double m;
	double pivot = local_matA[c][c];

	if (iter_begin == c) {
		++iter_begin;
		++matB;
	}

	if(-0.0000001<pivot && pivot<0.0000001)
		return;

	for(i=iter_begin; i<iter_end; ++i, ++matB){
		matA = origA[i]+c;
		matAc = origA[c]+c;
		m = *matA / pivot;
		for(j = n-c ; j>0 ; --j, ++matAc, ++matA){
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);
		}
		*matB = *matB - m* bC;
	}
}

void thread_forward1 (int id, int c, double** local_matA, double* local_matB) // id : thread id, c : (pivot) column
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA;
	register double* matAc;
	double* matB = local_matB+iter_begin;
	register double** origA = local_matA;
	register unsigned int i;
	register unsigned int j;
	double bC = local_matB[c];
	register double m;
	double pivot = local_matA[c][c];

	if (iter_begin == c) {
		++iter_begin;
		++matB;
	}

	if(-0.0000001<pivot && pivot<0.0000001)
		return;

	for(i=iter_begin; i<iter_end; ++i, ++matB){
		matA = origA[i]+c;
		matAc = origA[c]+c;
		m = *matA / pivot;
		for(j = n-c ; j>5 ; --j, ++matAc, ++matA){
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

		}
		*matA = *matA - m*(*matAc);

		*matB = *matB - m* bC;
	}
}

void thread_forward2 (int id, int c, double** local_matA, double* local_matB) // id : thread id, c : (pivot) column
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA;
	register double* matAc;
	double* matB = local_matB+iter_begin;
	register double** origA = local_matA;
	register unsigned int i;
	register unsigned int j;
	double bC = local_matB[c];
	register double m;
	double pivot = local_matA[c][c];

	if (iter_begin == c) {
		++iter_begin;
		++matB;
	}   

	if(-0.0000001<pivot && pivot<0.0000001)
		return;

	for(i=iter_begin; i<iter_end; ++i, ++matB){
		matA = origA[i]+c;
		matAc = origA[c]+c;
		m = *matA / pivot;
		for(j = n-c ; j>5 ; --j, ++matAc, ++matA){
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

		}
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		*matB = *matB - m* bC;
	}
}


void thread_forward3 (int id, int c, double** local_matA, double* local_matB) // id : thread id, c : (pivot) column
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA;
	register double* matAc;
	double* matB = local_matB+iter_begin;
	register double** origA = local_matA;
	register unsigned int i;
	register unsigned int j;
	double bC = local_matB[c];
	register double m;
	double pivot = local_matA[c][c];

	if (iter_begin == c) {
		++iter_begin;
		++matB;
	}

	if(-0.0000001<pivot && pivot<0.0000001)
		return;

	for(i=iter_begin; i<iter_end; ++i, ++matB){
		matA = origA[i]+c;
		matAc = origA[c]+c;
		m = *matA / pivot;
		for(j = n-c ; j>5 ; --j, ++matAc, ++matA){
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

		}
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		*matB = *matB - m* bC;
	}
}

void thread_forward4 (int id, int c, double** local_matA, double* local_matB) // id : thread id, c : (pivot) column
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA;
	register double* matAc;
	double* matB = local_matB+iter_begin;
	register double** origA = local_matA;
	register unsigned int i;
	register unsigned int j;
	double bC = local_matB[c];
	register double m;
	double pivot = local_matA[c][c];

	if (iter_begin == c) {
		++iter_begin;
		++matB;
	}

	if(-0.0000001<pivot && pivot<0.0000001)
		return;

	for(i=iter_begin; i<iter_end; ++i, ++matB){
		matA = origA[i]+c;
		matAc = origA[c]+c;
		m = *matA / pivot;
		for(j = n-c ; j>5 ; --j, ++matAc, ++matA){
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

			--j; ++matAc; ++matA;
			*matA = *matA - m*(*matAc);

		}
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		--j; ++matAc; ++matA;
		*matA = *matA - m*(*matAc);

		*matB = *matB - m* bC;
	}
}

void thread_make_leading_one0 (int id, int c,double** local_matA)
{
	unsigned int iter_begin = c + (id*(n-c))/nThreads;
	register unsigned int iter_end = c + ((id+1)*(n-c))/nThreads;
	register double* matA = local_matA[c]+iter_begin;
	double cc = local_matA[c][c];
	register unsigned int i;
	if (iter_begin == c){
		++iter_begin;
		++matA;
	}
	for (i=iter_begin; i<iter_end; ++i, ++matA){
		*matA = *matA / cc;
	}
	if (id == nThreads-1){
		matB[c] = matB[c] / cc;
	}
}

void thread_backward0 (int id, int c, double** local_matA, double* local_matB)
{
	register double** matA = local_matA;
	unsigned int iter_begin = (id*c)/nThreads;
	register unsigned int iter_end = ((id+1)*c)/nThreads;
	register double* matB = local_matB+iter_begin;
	register double bC = local_matB[c];
	register unsigned int i;

	for(i = iter_begin; i < iter_end; ++i, ++matB){
		*matB = *matB - matA[i][c] * bC;
		matA[i][c] = 0.;
	}
}

//TODO: implement it!
	void
thread_main (int id)
{
	unsigned int maxIdx;
	unsigned int l,k;
	double maxValue;
	unsigned int* local_matMax;
	/* you may remove the folloing line */
	// printf("Entering thread%d \n", id);
	for(k = 0; k < n; ++k){

		//search
		matMax[id] = thread_search0(id, k,matA);
		barrier();

		local_matMax = matMax;
		maxIdx = *local_matMax;
		maxValue = ABS(matA[maxIdx][k]);
		for(l = nThreads; l > 0; --l, ++local_matMax){
			double temp;
			temp = ABS(matA[*local_matMax][k]);
			if(maxValue < temp){
				maxIdx = *local_matMax;
				maxValue = temp;
			}
		}
#ifdef __DEBUG              
		print_matrix(matA, n);
#endif

		barrier();
		//swap


		thread_swap0(id,k,maxIdx,k,matA);
		/*
		   if (id==0){
		   double* rowtmp;
		   rowtmp = *(matA+k);
		 *(matA+k)= *(matA+maxIdx);
		 *(matA+maxIdx)= rowtmp;
		 }else if (id==1){
		 double tmp;
		 tmp = matB[k];
		 matB[k] = matB[maxIdx];
		 matB[maxIdx] = tmp;
		 }
		 */

#ifdef __DEBUG
		printf("-------------------------------------------\n");
		printf("after thread_swap(%d,%d,%d,%d) with maxValue=%.5lf\n", id, k, maxIdx, k, maxValue);
		print_matrix(matA, n);
#endif
		barrier();

		//forward subtact
		thread_forward[(n-k)%5](id,k,matA,matB);
#ifdef __DEBUG
		printf("-------------------------------------------\n");
		printf("after thread_forward(%d,%d)\n", id, k);
		print_matrix(matA, n);
#endif
		barrier();

		thread_make_leading_one0(id, k,matA);
		barrier();
		matA[k][k] = 1.;
	}

	//backward subtract
	for(k = n; k > 0; --k){
		thread_backward0(id,k,matA,matB);
		barrier();
	}

	// printf("Synced thread%d \n", id);

}


void initMat(int n)
{
	register unsigned int i, j;

	/* allocate memory for three matrices */
	matA = (double **)malloc(n*sizeof(double *));
	initA = (double **)malloc(n*sizeof(double *));
	for(i=0; i<n; i++){
		matA[i] = (double *)malloc(n*sizeof(double));
		initA[i] = (double *)malloc(n*sizeof(double));
	}

	matB = (double *)malloc(n*sizeof(double));
	initB = (double *)malloc(n*sizeof(double));
	matMax = (unsigned int *)malloc(nThreads*sizeof(int));

	/* assign the natural numbers to the elements in row major order */
	for(i=0; i<n; i++) {
		for(j=0; j<n; j++){
			matA[i][j] = drand48();
			initA[i][j] = matA[i][j];
		}
	}

	for(i=0; i<n; i++) {
		matB[i] = drand48();
		initB[i] = matB[i];
	}

#ifdef __DEBUG
	/* for debugging */
#endif
}

//TODO : erase it if unnecessary.
/* for debugging */
void checkResult(int n)
{
	register unsigned int i, j;
	double r=0.;
	double *ver = (double *)malloc(n*sizeof(double));
	/*
	   for (i=0;i<n;++i){
	   for (j=0;j<n;++j){
	   printf("%.5lf ", initA[i][j]);
	   }
	   printf("| %.5lf = %.5lf\n", matB[i], initB[i]);
	   }
	   printf("******************************************************************\n");
	   for (i=0;i<n;++i){
	   for (j=0;j<n;++j){
	   printf("%.5lf ", matA[i][j]);
	   }
	   printf("\n");
	   }
	 */
	for (i=0;i<n;++i){
		ver[i] = 0.;
		for (j=0;j<n;++j){
			ver[i] = ver[i] + initA[i][j]*matB[j];
			//initB[i] = initB[i]-initA[i][j]*matB[j];
		}
		r = r + (ver[i]-initB[i])*(ver[i]-initB[i]);
		//r = r + initB[i]*initB[i];
	}
	r = sqrt(r);

	printf("the result is ... \n");
	/*   for (i=0;i<n;++i){
		 printf("%.5lf %.5lf\n", ver[i], initB[i]);
		 }
		 printf("\n");
	 */
	printf("Residual is ... %lf\n", r);
	//printf("Success\n");
}


int main(int argc, char *argv[])
{
	struct timeval start, end;
	double elapsed_time = 0.0;
	unsigned int r, i;
	pthread_t *tid;
	pthread_attr_t attr;
	unsigned int j, l;

	if (argc != 3) {
		printf("Usage: numThreads sizeofMatrix\n");
		exit(1);
	}

	nThreads = atoi(argv[1]);
	n = atoi(argv[2]);

	/* Allocate thread ID's: */
	tid = (pthread_t *)malloc(sizeof(pthread_t *) * nThreads);
	assert (tid);

	/* Initialize thread attributes */
	r = pthread_attr_init (&attr);
	assert (!r);
	r = pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM);
	assert (!r);

	/* Initialize matrices */
	initMat(n);
	init_barrier(nThreads);

	/* initialize function array */
	thread_forward[0] = thread_forward0;
	thread_forward[1] = thread_forward1;
	thread_forward[2] = thread_forward2;
	thread_forward[3] = thread_forward3;
	thread_forward[4] = thread_forward4;

	/* Start timing */
	printf("Entering parallel execution\n");
	gettimeofday(&start, NULL);

	/* Fork threads */
	for ( i=1; i < nThreads; i++ ) {
		r = pthread_create (&tid[i], &attr, (void *(*)(void *))thread_main, (void *)i);
		assert (!r);
	}

	/* Call thread_main */
	thread_main (0);

	/* Wait for threads to complete */
	for ( i=1; i < nThreads; i++ ) {
		r = pthread_join (tid[i], 0);
		assert (!r);
	}


	/* End timing */
	gettimeofday(&end, NULL);
	printf("Exiting parallel execution\n");

	printf("numThreads=%d n=%d\n", nThreads, n);

	elapsed_time = (end.tv_sec-start.tv_sec)+(double)((end.tv_usec-start.tv_usec))/1000000;
	printf("Elapsed time = %.5f secs.\n", elapsed_time);

	checkResult(n);

	/* Free thread attributes and ids */
	r = pthread_attr_destroy (&attr);
	assert (!r);
	free(tid);
}


